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I.  INTRODUCTION 


Robots  have  long  been  in  man’s  thinking.  From  the  early  science  fiction 
writers  to  today’s  cartoons,  the  idea  of  robots  that  operate  on  their  own  has 
fascinated  us.  There  have  been,  and  still  are,  many  research  programs  concerned 
with  realization  of  an  autonomous  robot.  These  programs  include  wheeled 
machines,  tracked  vehicles,  and  legged  vehicles. 

There  are  many  uses  and  advantages  to  each  type  of  robotic  machine.  Legged 
vehicles  .or  walking  machines  have  an  advantage  over  wheeled  or  tracked  vehicles 
in  that  they  can  traverse  a  greater  variety  of  terrains.  A  walking  machine  can 
traverse  terrain  that  is  heavily  wooded,  can  walk  over  muddy  terrain,  and  can 
negotiate  obstacles  that  a  wheeled  or  tracked  vehicle  may  not  be  able  to 
overcome.  However,  before  any  robotic  machine  can  travel  anywhere  by  itself,  it 
has  to  be  able  to  see  where  it  is  going  and  decide  where  it  wants  to  go  based  on 
what  it  sees.  Once  all  of  these  research  areas  have  been  mastered,  widespread 
application  of  autonomous  robots  may  follow. 

A.  GOALS 

The  goal  of  this  thesis  is  to  explore  methods  to  obtain  accurate  terrain 
altitude  information  that  may  be  used  by  some  type  of  route  planning  program 
for  an  autonomous  vehicle.  The  process  of  obtaining  these  altitude  values  will 


include  some  type  of  filtering  of  noisy  information  and  correction  of  errors  in  the 
vehicle’s  location  information.  The  sensors  involved  in  this  process  are  assumed 


to  be  some  type  of  inertial  navigation  system  (INS)  and  an  optical  radar  scanner. 

The  vehicle  vised  as  a  reference  for  setting  up  the  model  used  in  this  thesis  is 
the  Ohio  State  University  Adaptive  Suspension  Vehicle  (ASV),  which  is  a  six¬ 
legged  walking  machine.  The  ASV  is  equipped  with  an  optical  terrain  scanner 
manufactured  by  the  Environmental  Research  Institute  of  Michigan  (ERIM). 
[Ref.  1] 

A  secondary  purpose  of  this  thesis  is  to  develop  a  simulation  model  for  the 
optical  scanner  and  terrain  that  will  be  used  in  the  testing  of  the  techniques 
presented  in  the  thesis.  These  models  could  also  be  used  in  future  studies  that 
involve  a  scanner  and  terrain. 


B.  ORGANIZATION 

Chapter  II  presents  a  discussion  of  work  dealing  with  early  robotic  systems  as 
well  as  current  projects  under  development.  The  optical  terrain  scanner  used  as  a 
reference  in  this  thesis  is  discussed  in  some  detail.  Finally,  a  general  discussion  on 
regression  analysis  and  Kalman  filtering  are  presented. 

The  reason  for  obtaining  an  accurate  terrain  altitude  map  is  discussed  in 
Chapter  III.  A  detailed  discussion  is  presented  on  how  the  inertial  navigation  and 
optical  scanner  data  are  mathematically  manipulated  to  obtain  the  x  and  y 
coordinates  and  the  terrain  altitude  of  the  terrain  being  scanned.  Finally,  a  more 
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detailed  discussion  of  K&lm&n  filtering  and  regression  analysis  as  they  apply  to 
this  thesis  are  presented. 

Chapter  IV  presents  the  simulation  models  used  for  the  terrain  and  the 
optical  scanner.  Also,  flow  charts  showing  how  data  is  handled  during  the 
simulation  runs  are  included. 

Finally,  Chapter  V  discusses  the  simulation  experiments  conducted  and  their 
results.  The  last  chapter,  Chapter  VI,  puts  forward  some  conclusions  about  the 
work  presented  in  this  thesis  followed  by  some  recommendations  for  future  work. 


II.  REVIEW  OF  PREVIOUS  WORK 


A.  INTRODUCTION 

For  many  years,  man  has  been  working  toward  the  goal  of  making  an 
autonomous  robot.  However,  before  a  robot  or  machine  can  be  truly  autonomous, 
it  has  to  have  some  way  of  seeing  the  world  in  which  it  is  to  operate.  In  this 
chapter,  past  robotic  systems  and  their  vision  systems  are  reviewed  along  with 
ways  of  handling  the  data  from  these  vision  systems. 

B.  MOBILE  ROBOT  SYSTEMS 

1.  Early  Robot  Systems 

Inventors  have  been  fascinated  for  centuries  with  the  idea  of  creating 
machines  that  act  like  animals  or  human  beings.  The  earliest  of  these  were 
"clockwork"  machines  like  the  "artificial  duck"  built  by  Jacques  de  Vaucanson  in 
the  1730’s,  and  Baron  Wolfgang  von  Kempenlen’s  chess-playing  automaton  of  the 
late  eighteenth  century  (later  proved  a  hoax).  Clockwork  machines,  as  the  name 
implies,  were  based  on  complex  mechanical  gears  and  linkages  which  provided  the 
timing  and  required  movements  for  the  machine.  Until  the  advent  of  digital 
computers  and  component  miniaturization  technology,  robots  remained  little 
more  than  mechanical  novelties.  [Ref.  2:  pp.  254-264] 


In  the  1960’s,  researchers  at  Johns  Hopkins  University  built  what  could 
possibly  be  considered  the  first  truly  autonomous  mobile  robot.  The  sole  activity 
of  this  robot  was  to  navigate  the  hallways  of  the  laboratory  "searching"  for 
electrical  power  outlets  to  keep  its  batteries  charged.  Special  purpose  circuitry 
(rather  than  a  programmable  computer)  was  used  to  direct  the  robot’s  actions, 
and  information  from  an  elementary  sonar  system  kept  the  robot  centered 
between  the  walls.  Although  simplistic,  the  machine  demonstrated  an  ability  to 
interact  with  its  enviroment  and  to  sustain  itself  through  its  own  actions  (i.e. 
searching  for  outlets  and  recharging  its  batteries).  [Ref.  2:  pp.  254-264] 

The  next  major  step  in  autonomous  robot  development  was  accomplished 
in  1969  at  the  Stanford  Research  Institute  (SRI).  A  mobile  robot  dubbed 
"Shakey"  was  linked  to  a  digital  computer  via  radio  giving  it  the  much  needed 
additional  "brainpower"  to  more  fully  interact  with  its  enviroment  and  solve 
relatively  complex  problems.  Using  a  television  camera  as  a  sensor  and 
hierarchical  software  control,  Shakey  was  able  to  negotiate  obstacles  and 
accomplish  simple  path  planning  when  moving  from  one  location  to  another. 
This  first  attempt  at  machine  interaction  with  an  uncertain  enviroment 
encountered  many  problems.  Rather  complex  image  processing  algorithms  were 
required  to  discern  potential  obstacles  and  complicated  computer  programs  were 
needed  to  map  the  obstacles  onto  a  spatial  grid  coordinate  system  that  the  robot 
could  "understand".  Shakey  kept  track  of  its  own  position  through  a  crude  "dead 
reckoning"  system  in  which  sensors  counted  the  number  of  rotations  of  the  robot’s 


wheels  to  determine  how  far  it  had  moved  from  its  initial  starting  position.  It  was 
soon  discovered  that,  while  multiple  sensors  can  provide  a  more  comprehensive 
representation  of  the  enviroment,  coordinating  the  incoming  data  from  those 
sensors  represents  a  major  hurdle.  For  instance,  on  the  Shakey  robot,  wheel 
slippage  caused  position  errors  which  in  turn  induced  grid  map  drift  and  resulted 
in  multiple  representations  of  individual  obstacles  cluttering  the  map.  [Ref.  3:  pp. 
10-15] 

For  all  its  problems,  the  Shakey  robot  remained  state-of-the-art  until  the 
mid-1970’s.  During  this  decade,  research  involving  incorporation  of  new 
technologies  into  autonomous  robots  continued.  Advances  in  integrated 
electronics  manufacturing  made  possible  relatively  powerful  single-board 
computers  and  smaller,  more  compact  sensor  systems.  Still,  many  of  the  robots 
built  during  this  time  frame  were  tethered  to  an  off-board  computer  either  via 
cables  or  radio  link.  Among  the  robots  of  this  period  were  the  Jet  Propulsion 
Laboratory’s  Mars  Rover,  the  Stanford  Cart,  and  France’s  Laboratoire 
d’Automatique  et  d’Analyse  des  Systemes’  HILARE  robot.  These  robots  all 
shared  the  common  goal  of  integrating  multiple  sensor  systems  and  developing 
algorithms  to  allow  autonomous  operation  in  uncertain  enviroments.  [Ref.  3:  pp. 
20-25] 

The  JPL  Rover  used  a  laser  range  finder,  stereo  TV  cameras,  tactile,  and 
proximity  sensors  to  observe  its  enviroment,  and  a  gyrocompass  and  optical 
encoders  on  the  wheels  to  keep  track  of  its  own  position.  Like  the  Shakey  robot, 


sensor  integration  and  error  propogation  problems  plagued  the  system  and,  due  to 
the  heavy  computing  requirements,  the  robot  was  dependent  on  its  mainframe 
computer  link.  [Ref.  3:  pp.  20-30] 

The  Stanford  Cart  employed  a  complex  image  processing  scheme  to 
improve  obstacle  recognition.  A  TV  link  to  an  off-board  computer  and  a 
moveable,  slide-mounted  camera  system  were  used  to  reduce  imaging  errors,  but 
this  system  proved  slow  and  very  sensitive  to  light  and  shadow  effects.  Like 
previous  robots,  the  Stanford  Cart’s  dead  reckoning  system  induced  errors  into 
the  system  which  it  could  not  overcome.  [Ref.  3:  pp.  20-25] 

The  French  Hilare  robot  incorporated  a  number  of  techniques  most  often 
associated  with  artificial  intelligence.  "Expert  system"  modules  worked  together, 
sharing  information  about  navigation,  obstacle  identification,  etc.,  under  a 
computing  hierarchy  of  onboard  microcomputers  linked  to  an  off-board 
minicomputer  and  mainframe.  The  Hilare  used  a  laser  range  finder  and  a 
prediction  algorithm  to  anticipate  what  the  obstacle  map  would  "look"  like  after 
the  robot  moved.  It  could  then  correct  its  perception  and/or  position  information 
to  merge  the  predicted  and  actual  image  information  and  thereby  alleviate  many 
of  the  errors  experienced  in  the  earlier  systems.  [Ref.  3:  pp. 20-25] 

A  departure  from  wheeled  vehicles  is  the  Ohio  State  University  (OSU) 
Hexapod.  The  Hexapod  is  a  six-legged  walking  machine  which  first  operated  in 
1977.  Although  the  OSU  Hexapod  is  strictly  a  laboratory  robot,  a  truly 
autonomous  walking  machine,  like  its  animal  counterpart  in  nature,  would  be 


ideally  suited  to  traversing  rough,  unpredictable  terrain.  The  OSU  machine’s  legs 
are  controlled  to  provide  static  stability;  that  is,  at  least  three  legs  are  on  the 
ground  providing  a  stable  supporting  tripod  for  the  robot  at  all  times.  The 
robot’s  sensors  include  stereo  TV  cameras  and  motion  sensors  to  maintain  stable 
body  position  while  moving  or  climbing.  While  sophisticated  computer  routines 
automate  many  of  the  Hexapod’s  balance,  leg  movement,  and  coordination 
functions,  the  Hexapod  remains  tethered  to  its  external  power  supply  and 
minicomputer  and  relies  on  human  interaction  for  navigation  and  for  some  foot- 
placement  decisions.  [Ref.  4:  pp.  3-17] 

2.  Autonomous  Land  Vehicles 

The  Autonomous  Land  Vehicle  (ALV)  is  the  newest,  and  thus  far  the 
most  successful,  of  the  various  autonomous  wheeled  robots  which  have  been  tested 
to  date.  Designed  for  the  Defense  Department’s  Advanced  Research  Projects 
Agency  (DARPA),  the  vehicle  is  a  hydraulically  powered,  eight-wheeled  platform 
equipped  with  the  computers  and  sensors  required  to  negotiate  unfamiliar 
territory  without  human  intervention.  The  ALV’s  mission  is  conceptually  more 
difficult  than  the  robots  previously  described  since  it  is  designed  to  operate 
outdoors  in  a  much  more  unpredictable  environment.  The  ALV’s  sensors  include  a 
TV  camera  and  optical  radar  for  sensing  the  enviroment,  and  an  inertial 
navigation  system  for  position  information.  During  initial  testing,  the  sensor  data 
was  successfully  integrated  and  processed,  providing  the  vehicle  with  "road 
following"  information  able  to  keep  it  between  ditches.  [Ref  5] 


Another  ALV  was  built  in  1985  by  FMC  Corporation.  This  vehicle  is  an 
armored  personnel  carrier  which  is  a  tracked,  instead  of  wheeled  vehicle.  This 
ALV  has  an  inertial  navigation  system,  a  vehicle  control  computer,  and  a  sonic 
imaging  sensor.  The  architecture  of  the  FMC  vehicle  consists  of  5  subsystems 
called  a  Planner,  Observer,  Mapmaker,  Pilot,  and  Vehicle  Control.  In  general, 
the  Planner  plans  the  route  of  the  ALV  using  preloaded  digitized  maps  of  the 
local  terrain.  The  Observer  uses  the  sensors  input  to  create  an  obstacle  map,  then 
the  Mapmaker  generates  a  pilot  map  using  the  information  from  the  Planner  and 
the  obstacle  map.  The  Pilot  uses  the  pilot  map  and  guides  the  vehicle  along  an 
optimum  path  by  passing  instructions  to  the  Vehicle  Control  subsystem.  The 
first  test  of  the  FMC  ALV  was  conducted  in  1985  in  which  it  successfully  avoided 
obstacles  and  performed  path  execution  at  a  speed  of  5  mph.  [Ref.  6:  pp.  14-23] 

3.  DARPA  Adaptive  Suspension  Vehicle 

A  more  advanced  six-legged  walking  machine,  called  the  Adaptive 
Suspension  Vehicle  (ASV),  is  currently  undergoing  test  and  evaluation  at  Ohio 
State  University.  The  ASV  differs  from  the  OSU  Hexapod  in  that  it  is  completely 
self-sufficient  (no  external  power  or  computing  required).  The  ASV  uses  an 
internal  combustion  engine  to  provide  power  to  its  hydraulic  systems  and  on¬ 
board  computers.  The  ASV  carries  its  own  computers  that  control  leg  movement 
and  stability,  and  provide  vision  information.  The  vision  system  consists  of  an 
optical  radar  mounted  on  top  of  the  control  cab.  At  present,  the  ASV  requires  an 
on-board  human  operator  to  plan  and  control  the  motion  of  its  body  using  a 
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three-axis  control  stick.  With  continuing  research,  the  ASV  could  become  the 
first  autonomous  legged  walking  machine.  [Ref.  1:  pp.  1-60] 

C.  DESCRIPTION  OF  OPTICAL  RADAR 

1.  Physical  Description 

The  ASV  is  equipped  with  an  optical  terrain  scanner.  The  optical 
scanner  is  manufactured  by  the  Environmental  Research  Institute  of  Michigan 
(ERIM).  The  scanner  is  26  inches  wide,  12.5  inches  high  and  weighs  75  pounds 
[Ref.l:  pg.  35].  The  optical  scanner  consists  of  a  scanning  mechanism,  transmitter 
optical  train,  and  receiver  optical  train.  The  scanning  mechanism  consists  of  a 
nodding  mirror  and  a  four-sided  polygon  mirror  which  combine  to  scan  up  and 
down  and  left  and  right.  The  transmitter  optical  train  consists  of  a  GaAlAs  laser 
diode,  collimating  lens,  anamorphic  prism  pair  and  a  beam  expansion  telescope. 
[Ref.  7:  pp.  3-4], 

2.  Performance  Data 

The  optical  scanner  has  a  scan  rate  of  2  Hertz.  The  horizontal  field  of 
scan  is  +40  to  -40  degrees  in  128  increments  while  the  vertical  field  of  scan  is  from 
-15  to  -75  degrees  elevation  in  128  increments.  The  instantaneous  field  of  view  is 
0.5  degrees  and  the  range  resolution  is  0.125  feet.  The  maximum  horizontal  range 
is  32  feet.  [Ref.  7:  pp.  2-3] 

Due  to  various  factors  such  as  weather,  obstacles  and  equipment  errors, 
the  range  value  returned  by  the  ERIM  scanner  is  not  exact  [Ref.  7:  pg.  5].  The 


ASV  has  or  will  have  gyro  or  magnetic  compasses  for  azimuth,  elevation  and  roll 
inputs  along  with  an  altimeter  for  altitude  information  and  geoposition  satellite 
(GPS)  inputs  for  horizontal  position.  Even  though  the  ASV  will  be  able  to 
receive  position  inputs  from  GPS  and  the  altimeter,  they  are  inaccurate.  That  is, 
although  the  gyro’s  give  angles  within  fractions  of  a  degree,  the  errors  associated 
with  GPS  and  the  altimeter  are  of  the  order  of  tens  of  feet;  therefore,  x,  y,  and  z 
need  co  be  corrected.  Detailed  information  regarding  these  two  sources  of  terrain 
mapping  error  was  not  available  at  the  time  of  writing  of  this  thesis. 

D.  ESTIMATION  TECHNIQUES 
1 .  Regression  Analysis 

In  regression  analysis,  a  number  of  noisy  measurements  of  a  variable  are 
used  to  approximate  a  functional  relationship.  Using  the  notation  of  Ref.  8,  if  the 
measured  variable  of  the  system  is 

*o(0  *  r(‘.?o)  +  «(*)  (2.1) 

then  the  objective  of  regression  analysis  is  to  find  a  vector,  e  ,  which  approximates 
the  true  parameter  vector,  e0,  in  the  presence  of  the  measurement  error,  <  . 
Typically,  this  is  accomplished  by  minimization  of  some  type  of  sum  squared  error 
function. 

If  the  response  of  the  system  under  consideration  can  be  represented 


linearly  as 


i0  -A70  +  1 


(2.2) 


where  f#  is  a  vector  of  samples  of  y0,  A  is  the  coefficient  matrix,  ?„  is  the  true 
parameter  vector,  and  t  is  a  random  error  vector,  then  the  sum  squared  error 
function  ,  *,  can  be  defined  as 


*  “  fro  -  Afl'fro  -  A ?) 


where  ?  is  the  trial  parameter  vector.  To  minimize  *  ,  the  partial  derivatives  of  9 
with  respect  to  each  component  of  t  are  equated  to  zero.  Applying  this  to  Eq. 
(2.3),  the  result  is 


—  -  V*  -  -lArh  +  1AT At  -  0 

a? 


ATA?~  ati0 


Solving  Eq.  (2.5)  for  the  least  squares  estimate  of  the  true  parameter  vector,  the 


result  is 


?-  \ATA}~lATj0 


This  is  the  least  squares  parameter  estimate  of  the  true  parameter,  ?.  [Ref.  8:  pp. 
65-68] 

2.  Iterative  Methods 

The  Kalman  filter  is  an  iterative  method  used  to  obtain  an  optimal 
estimate  of  a  variable  in  an  environment  that  has  noise  present.  Unlike  regression 


analysis,  in  using  a  Kalman  filter,  it  is  not  necessary  to  store  past  measurements 
for  present  or  future  computations.  For  the  purposes  of  this  discussion,  the 
notation  in  Ref.  9  is  used.  It  is  assumed  the  reader  has  some  knowledge  of 
Kalman  filtering. 

Assume  a  system  is  described  by 

2k  =  **-iz*-i  +  1  (2-7) 

and 

-  Hk2k  +  1k  (2.8) 

where  2k  is  the  state  variable  at  time  tk,  **_,  is  the  transition  matrix  at  time  tk_ „ 

ak_k  and  vt  are  the  random  noise  vectors  with  zero  mean  and  covariances  Qk  and 

Rk,  respectively,  2k  are  the  measurements,  and  Hk  is  the  observation  matrix.  An 

updated  estimate  of  the  state,  l4(+),  based  on  the  measurement  2k  and  the  past 

estimate,  #*(-),  can  be  obtained  from  the  recursive  form 

*»(+)  -  +  Kk2k  (2.9) 

where  Kk  and  Kk  are  the  time-varying  weighting  matrices  (Kalman  gain  matrices). 

[Ref.  9:  pp.  60-110] 

If  2k  denotes  the  estimation  error,  such  that 


**(+)  “  2*  +  ?*(+) 

(210) 

2*H  -  2*  +  z*R 

(2.11) 

then  Eqs.  (2.8-2.11)  yield 
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»,(+)  -  |*.  +  KtH,  -  /If,  +  Kt !,(-)  +  /f,f, 


(2.12) 


By  definition,  £[?tj  -  0  and  if  E[3k(-)\  -  0,  this  estimator  will  be  unbaised  for  any  lk 


Kk  +  KkHk  -  1  =  0 


(2.13) 


Thus,  Eqs.  (2.10)  and  (2.12)  become 


2*(+)  -  2*R  +  Kk\lk  -  Hjk(-)) 


(2.14) 


f4(+)  -  (/  -  KkHk)2k(~)  +  KkVk  . 


(2.15) 


By  definition,  the  error  covariance  matrix,  Pk ,  is  given  by 


P.M  -  P|f,(+)f,(+)r] 


(2.16) 


P.(-)  -  £lf.(-)f,(-)r] 


(2.17) 


If  it  is  assumed  that  the  measurement  errors  axe  uncorrelated,  then 


E\K(-)*l\  ‘  E\ljkUT\  -  0 


(2.18) 


Applying  Eq.  (2.12)  to  Eqs.  (2.16)  and  (2.17) 


Pt(  +  )  =  (/  -  *4ff4)P4(-)(/  -  KkHk)T  +  KkRkKTk 


(2.19) 


The  Kalman  gain  matrix,  Kk,  is  defined  as 


Kt  =  Pk[-)HTk\HkPk(-)HTk  +  Rk'-1 


(2.20) 


Substituting  Eq.  (2.20)  into  Eq.  (2.19) 
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«  |>,  »t,  , 


/»*(+)  =  [1  -  K^P^-)  . 


(2.21) 


From  Eqs.  (2.14),  (2.20)  and  (2.21),  a  recursive  filter  can  be  set  up  to  obtain  the 
optimal  estimates  of  f4(+)  (Ref.  9:  pp.  60-110). 


E.  SUMMARY 

As  has  been  discussed  in  this  chapter,  there  have  been  in  the  past  and  are 
now  in  progress,  many  projects  to  develop  autonomous  vehicles.  To  become 


autonomous,  such  a  vehicle  ought  to  have  a  vision  system  and  one  such  system 


in.  PROBLEM  STATEMENT  AND  PROPOSED  SOLUTION  METHOD 


A.  INTRODUCTION 

Before  a  vehicle  can  become  autonomous,  it  needs  to  know  what  the  terrain  it 
is  operating  in  looks  like  then  be  able  to  pick  a  path  it  wants  to  travel  along.  In 
this  chapter,  some  path  selection  techniques  are  discussed  in  general  terms  along 
with  some  detailed  discussion  on  filtering  of  terrain  data. 

For  the  remainder  of  this  thesis,  when  reference  is  made  to  an  optical  radar 
scanner, -the  ERIM  discussed  in  Section  C  of  Chapter  II  is  assumed  to  be  the  one 
in  use.  This  assumption  will  show  up  in  performance  data  assumed  for  the  data 
conversions  and  simulation. 


B.  IMMEDIATE-AREA  TERRAIN  SENSING  AND  PATH  SELECTION 

The  need  for  information  about  the  terrain  in  the  immediate  area  of  a 
walking  machine  in  crucial.  As  long  as  the  machine  has  a  human  driver,  he  can 
navigate  the  terrain  manually,  picking  where  to  walk  and,  if  necessary,  placing 
the  feet  in  appropriate  positions.  However,  before  a  walking  machine  can  become 
truly  autonomous,  it  needs  to  be  able  to  navigate  on  its  own  and  pick  its  own 
path  to  walk  along.  Moreover,  even  in  the  case  of  a  manned  vehicle,  it  is  highly 
desirable  that  the  driver  be  concerned  only  with  control  of  the  body  of  the  vehicle 
with  foothold  selection  being  accomplished  automatically  from  vision  data. 
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There  have  been  many  approaches  and  schemes  for  solving  the  problem  of 
selecting  adequate  footholds  and  avoiding  obstacles.  The  Autonomous  Land 
Vehicle  (ALV)  uses  a  vision  system  that  is  a  combination  of  a  color  video  TV 
camera  and  the  ERIM  optical  radar  scanner  [Ref.  5:  pp.  19-23].  The  color  camera 
detects  differences  in  color  between  the  road  and  off-road  areas,  while  the  optical 
scanner  is  used  strictly  to  detect  differences  in  smoothness  between  the  two  areas. 
A  major  disadvantage  of  this  system  is  that  the  color  camera  is  greatly  affected  by 
changes  in  lighting  and  weather  conditions. 

Another  obstacle  avoidance  system  is  used  in  the  FMC  ALV.  In  that  system, 
the  Observer  uses  information  from  a  sonic  image  sensor  to  detect  obstacles  not 
already  known  on  the  stored  digitized  map  and  then  creates  an  obstacle  map. 
The  sonic  image  sensor  is  a  phased  array  sensor  that  has  a  range  of  32  meters  and 
an  arc  of  120  degrees.  The  sensor  utilizes  sonic  waves  and  produces  a  map  every 
0.2  seconds.  [Ref.  6:  pp.  14-23] 

With  a  vision  system  consisting  of  just  an  optical  radar  terrain  scanner,  the 
problem  of  autonomous  navigation  becomes  even  more  difficult.  Before  an 
"optimal"  path  can  be  picked  to  walk  along,  accurate  information  about  the 
terrain  altitude  in  the  immediate  area  has  to  be  known.  Once  this  information  is 
available,  a  scheme  can  be  used  to  decide  what  areas  can  be  walked  over  and 
what  areas  cannot. 

One  possible  terrain  classification  scheme  was  developed  by  Poulos  in  Ref.  10. 
In  his  work,  a  least  squares  quadratic  surface  is  fitted  to  a  surface  pixel  and  its 


eight  neighbors.  From  the  x,  y,  z  information  of  each  pixel,  he  uses  the  gradient 
vector  and  Hessian  matrix  and  solves  for  the  eigenvalues  of  the  Hessian  matrix. 
From  these  solutions,  he  classifies  each  pixel  as  a  saddle,  depression,  ridge,  plane, 
valley,  hill  or  pass  [Ref.  10:  pp.  50-69].  To  further  refine  the  terrain  picture,  he 
also  takes  into  account  the  slope  associated  with  a  pixel.  Depending  on  what 
criteria  is  set  for  what  is  a  safe  slope  to  traverse,  each  pixel  is  also  classified  as 
being  level  or  having  a  safe  or  unsafe  slope.  The  property  of  being  level,  safe 
slope,  or  unsafe  slope  is  called  the  primary  terrain  cell  classification,  while  the 
above  mentioned  categories  derived  from  the  Hessian  matrix  are  called  the 
secondary  classification  [Ref.  10:pp. 70-78]. 

With  each  terrain  cell  classified,  it  becomes  possible  to  use  some  type  of 
artificial  intelligence  technique  to  select  the  ''optimal’'  path  to  traverse  [Refill]. 
The  only  problem  is  to  make  sure  that  the  terrain  information  provided  to  the 
classifying  scheme  is  the  best  and  most  accurate  possible. 

C.  TERRAIN  SENSING  USING  THE  OPTICAL  RADAR 

To  be  able  to  use  the  information  obtained  from  the  optical  scanner,  the  data 
must  first  be  converted  into  Cartesian  coordinates.  This  conversion  of  the  data 
will  allow  manipulation  of  any  prior  or  future  information  that  is  not  in  the  same 
data  format  as  the  incoming  data  [Ref.  7:  pp.  2-3].  In  order  to  convert  the  optical 
scanner  information  into  Cartesian  coordinates,  it  is  necessary  to  use  not  only  the 


scanner  information,  but  also  the  information  from  the  Inertial  Navigation  System 
(INS). 

To  do  the  scanner  data  conversion,  it  was  decided  to  model  the  INS  and 
optical  scanner  systems  as  a  nine  link  manipulator  and  to  use  the  Denavit- 
Hartenberg  (D-H)  transformation  for  this  purpose  [Ref  12:.pp.  36-41).  The  inputs 
from  the  INS  system  consist  of  an  x,  y,  z  translation  from  the  INS  origin,  and  the 
body  azimuth,  elevation  and  roll  angles.  The  inputs  provided  by  the  optical 
scanner  are  scanner  elevation  and  azimuth  angles  and  range  to  the  terrain.  The 
first  three  links  and  the  last  link  of  the  model  are  translational  transformations, 
while  the  others  are  rotational  transformations.  Figure  1  is  the  representation  of 
the  resulting  nine  link  manipulator.  Table  1  explains  what  each  value  in  Figure  1 
represents. 

In  drawing  Figure  1  and  setting  up  the  D-H  transformation  matrices,  the 
following  assumptions  were  made:  [Ref.  12:pp.36-4l] 

1.  The  reference  coordinate  system  for  the  ASV  body  is  positive  z  down,  positive 
x  out  of  the  front  of  the  vehicle,  and  positive  y  out  the  right  side. 

2.  The  2,_ ,  axis  lies  along  the  axis  of  motion  of  the  ith  joint  and  the  x.  axis  is 
normal  to  the  z,_,  axis. 

3.  When  possible,  the  direction  of  the  ith  twist  axis,  r,,  associated  with  a 
particular  link  in  the  manipulator  model  is  picked  to  make  the  twist  angle 
(a,.)  positive. 


Figure  1 

Nine  Link  Manipulator  Representation 


A 

A 

A 

A 

A 

A 

A 

A 

A 

ai 


displacement  from  the  INS  origin  in  the  z  direction 
displacement  from  the  INS  origin  in  the  x  direction 
displacement  from  the  INS  origin  in  the  y  direction 
ASV  body  azimuth  angle 
ASV  body  elevation  angle 
ASV  body  roll  angle 
scanner  elevation  angle 
scanner  azimuth  angle 
range  to  terrain 

distance  between  INS  origin  and  scanner  origin 
distance  between  scanner  mirrors 


The-  following  definitions  are  needed  for  the  discussion  of  the  D-H 
transformation  matrices  for  link  i:  [Ref.  12:pp.36-4l] 

1.  Link  length  (a,)  is  the  linear  displacement  from  the  inboard  motion  axis  to 
the  outboard  motion  axis  along  the  twist  axis,  (*,). 

2.  Twist  angle  (a,)  is  the  angular  displacement  of  the  outboard  motion  axis,  (z,), 
from  the  inboard  motion  axis,  (z,_,),  about  the  twist  axis,  (x,.). 

3.  Joint  displacement  (d,)  is  the  linear  displacement  of  the  outboard  twist  axis, 
(x,),  from  the  inboard  twist  axis,  (x,_j),  measured  along  the  inboard  motion 
axis, 


4.  Rotation  angle  (©,.)  is  angular  displacement  of  outboard  twist  axis,(x,),  from 
the  inboard  twist  axis,  (x,_,),  measured  about  the  inboard  motion  axis, 


Using  these  assumptions  and  definitions,  Table  2  shows  the  joint  and  link 
parameter  values  for  the  ASV  scanner  system.  The  values  in  Table  2  in 
parentheses  are  for  the  reference  configuration  shown  in  Figure  1  while  the  others 
are  fixed  values. 

Using  the  information  in  Table  2,  the  general  D-H  transformation  matrix,  Eq. 
(3.1),  can  be  used  to  form  each  link  as  follows: 


e0,  — ea,#©,.  so,*©,  ae©, 


<0,  cQjC©,  -10,(0,  a*©,. 


0  ia. 


In  Eq.  (3.1),  '~lA-  is  the  D-H  transformation  matrix  for  link  i  going  from  origin  (i- 
1)  to  origin  i.  Also,  e0,  represents  cos©,  and  «©,  represents  sin©,.  To  obtain  the 


TABLE  2 

LINK  AND  JOINT  PARAMETER  VALUES 


Cartesian  coordinates  of  the  terrain  at  the  end  of  the  scanner  beam,  each 
transformation  matrix  is  multiplied  in  order  as  shown  in  Eq.  (3.2). 

o.  o.  1.  j.  *  .  4.  s  .  e.  t.  *  . 

iAg  *  j4 j  x  Aj  x  A|  x  x  A ^  x  A^  x  A^  x  Ag  x  Ag 

In  order  to  simulate  the  scanning  of  terrain  on  the  graphics  computer,  the 
matrix  °At  which  specifies  the  position  of  the  ASV  body  is  computed  and  then 
multiplied  by  the  matrix  to  get  the  coordinate  transformation  for  the  beam 
tip.  Using  Eq.  (3.1)  and  Table  2,  the  following  results  are  obtained 

e4e5e6-t-«4«6  c4*5  e4e5«6— «4c6  rf- 
*4c5e6— e4#6  «4«5  «4e5*6+e4e6  d, 

(3.3) 

*5e8  -e5  i5«6  dl 

0  0  0  1 

and 

e7e8  -<7  c7j8  </Be7#8+o7e7 
#7c8  c7  «7#8  </9f7*8  +  a7«7 

(3.4) 

— «8  0  c8  4#c8 

0  0  0  1 

In  Eqs.  (3.1),  (3.3),  and  (3.4),  the  notation  has  again  been  abbreviated  so  that,  for 
example,  c7  stands  for  cos©T  and  s7  stands  for  sin©7.  Also,  the  origins  of  the 
scanner  and  the  INS  system  are  assumed  to  be  in  the  same  place,  thus  making  ae 
=  0.  While  this  is  in  fact  not  the  case  for  the  ASV,  this  assumption  simplifies  the 
computations  needed  to  evaluate  Eq.  (3.2)  and  at  the  same  time  has  no  effect  on 
the  validity  of  the  simulation  studies  carried  out  in  the  work  of  this  thesis. 


D.  PROPOSED  METHOD  OF  DATA  FILTERING 


Under  the  present  scheme  used  by  Ohio  State  University,  the  ASV  scans  the 
terrain  using  the  optical  radar  scanner,  and  using  whatever  information  the  INS 
provides,  a  terrain  map  is  calculated  and  stored.  On  all  successive  scans,  the 
terrain  map  is  recalculated  with  the  given  data,  and  the  resulting  new  terrain 
elevation  values  replace  the  old  ones.  Each  map  is  stored  using  the  x,  y,  and  z 
values  calculated  using  the  noisy  range  values  provided  by  the  optical  radar 
scanner,  which  makes  these  x,  y,  and  z  values  incorrect.  The  following  discussion 
presents  an  approach  to  correcting  for  the  z  value  errors  separately  from  the  x  and 
y  value  errors.  It  is  proposed  to  use  Kalman  filtering  for  the  z  values  and 
regression  analysis  for  the  x  and  y  values. 

1.  Stationary  Walker  Case 

In  the  case  where  the  ASV  is  standing  still,  the  only  significant  errors 
introduced  into  the  system  are  the  noisy  range  values  from  the  optical  radar 
which  in  turn  affect  the  calculated  values  of  the  altitude,  z.  In  this  case,  to  obtain 
the  optimal  values  of  the  altitude  for  each  terrain  cell,  Eqs.  (2.14),  (2.20)  and 
(2.21)  can  be  used.  If  the  assumption  is  made  that  Hk  in  Eq.  (2.14)  is  equal  to  1, 
then  Eq.  (2.14)  becomes 

?*<+)  -  ?*(“)  +  Kk\ik  -  ik(-)\  (3.5) 

where  ?*(+)  is  the  estimate  of  the  terrain  altitude  after  the  range  measurement. 
Also,  #t(— )  is  the  estimate  before  the  measurement  and  Kk  is  the  Kalman  gain 
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matrix.  Finally,  V  is  the  value  of  the  altitude  calculated  using  the  noisy  range 
measurement. 

From  Eq.  (2.20),  Pk(~)  is  defined  as  the  variance  of  the  error  before  the 
measurement  and  Rk  is  the  variance  of  the  measurement.  For  the  purposes  of  this 
problem,  Rk  is  the  same  as  the  variance  of  the  altitude  with  respect  to  the  range 
and  Pk(~)  is  the  total  variance  of  the  altitude.  Thus,  Rk  can  be  written  as 

**  -  a\  -  (3.0) 

orange 

where  a\M  is  the  variance  associated  with  each  scan  of  the  optical  radar  scanner. 
In  the  case  of  Pk(~),  there  is  not  only  a  variance  in  the  z  direction  but  there  is  also 
a  variance  in  the  x  and  y  calculations  due  to  the  noisy  range  values.  Thus,  Pk(~) 
can  be  written  as 

-  *]  +  (— )  *.*  +  ■  (3-7) 

dz  dy 

Using  Eqs.  (3.6)  and  (3.7),  Eqs.  (2.20)  and  (2.21)  can  be  written  as 

**  -  'LmH  (3-8) 

and 

p^)  -  -  "L-HI"'  •  (3-9) 

2.  Compensating  for  Walker  Motion 

When  the  ASV  is  walking,  or  when  there  is  an  INS  initialization  error, 
the  problem  becomes  one  of  computing  the  optimal  value  for  the  altitude  and 
ensuring  it  is  stored  in  the  correct  terrain  cell.  Due  to  the  inaccuracies  of  the 


altimeter  and  GPS  inputs  discussed  in  Section  C  of  Chapter  II,  it  is  necessary  to 
estimate  the  difference  in  the  horizontal  coordinates,  Ax  and  Ay.  For  this  part  of 
the  problem,  regression  analysis  will  be  used  to  select  the  correct  terrain  cell. 
Then  the  optimal  altitude  value  calculated  from  Kalman  filtering  will  be  stored. 
Two  methods  of  regression  analysis  will  be  discussed  for  use  in  this  problem:  the 
gradient  descent  method  and  a  grid  search  method, 
a.  Gradient  Descent  Method 

In  this  approach,  there  is  a  criterion  function,  *,  for  a  given  set  of 
parameters.  The  criterion  function  will  be  the  sum  squared  error  between  the 
range  returned  by  the  optical  radar  scanner,  J?,  and  the  range  computed  from  the 
internal  terrain  map  pre-stored  in  the  ASV  computers,  R.  From  this,  Eq.  (2.3) 
can  be  written  as 

♦  «  £(*  -  -  R)  (3.10) 

To  correct  for  the  x  and  y  coordinate  drift,  the  gradient  of  and 

the  Hessian  matrix,  H,  will  be  estimated  using  a  3  x  3  grid  around  the  INS  values 
of  the  x  and  y  coordinates.  Using  the  procedure  described  by  Poulus  in  Ref.  10, 
the  3x3  terrain  cell  mask  is  set  up  with  the  cell  of  interest  at  point  where  x=0 
and  y=0  with  its  associated  value  of  #0.  Table  3  shows  the  relative  distances  of 
the  cells  and  the  criterion  functions  associated  with  them. 

A  quadratic  function  can  be  fitted  to  the  cells  of  interest  as  a  function 


of  x  and  y  which  has  the  form 
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- +  2A‘  Ak  =  0 

3*. 


A  T Ak  =  AT*T  . 


(3.15) 


(3.16) 


Rearranging  terms,  Eq.  (2.6)  can  be  written  as 


k  -  \ATA}~lAT*7 


(3.17) 


The  gradient  of  the  terrain  cell  of  interest  is  [Ref.  10:  pg.  59] 


—  k^t  +  ktj 


(3.18) 


and  the  Hessian  matrix  is  [Ref.  10:  pg.60] 


*4 


2*s  *eJ 


(3.19) 


Applying  Eqs.  (3.18)  and  (3.19),  the  optimum  amount  of  Ax  and  Ay 


can  be  computed  as 


(Az,Ay)  =  fT1^ 


(3.20) 


which  leads  to  the  best  estimate  for  the  x  and  y  coordinates  of  the  terrain  cell  as 


~  +  ^Z’^INS  +  &1t) 


where  XINS  and  YINS  are  the  coordinates  supplied  by  the  INS  system. 


(3.21) 


In  order  to  be  able  to  compute  the  optimum  values  of  Ax  and  Ay,  the 
values  of  the  k’s  in  Eq.  (3.17)  have  to  be  computed.  Equation  (3.17)  can  be  re¬ 
written  as 


>,*%,**? 


where 


(3.22) 


k  =  \AT  A\~l  A1  $T  =  B*t 


fl=  [ATA]~lAT  .  (3.23) 

To  avoid  repeated  lengthy  matrix  computations,  Ref.  10  shows  B  to  be 
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b.  Grid  Search  Method 

For  this  search  method,  the  assumption  is  made  that  the  criterion 
function,  *,  is  definitely  reduced  when  a  sufficiently  small  step  is  taken  in  the 
direction  of  the  actual  terrain  cell.  Applying  this  to  the  terrain  cell  mask  in  Table 
3,  the  distances  between  each  cell  will  be  assumed  to  start  at  one  standard 
deviation,  a,  of  the  INS  system  estimate  of  x  and  y  instead  of  a  unit 
displacement.  After  each  computation  of  the  $’s  in  the  3x3  mask,  the  least 
value  *  is  moved  to  the  center  of  the  mask  and  the  coordinates  of  that  terrain  cell 
become  the  terrain  cell  of  interest  and  the  3x3  mask  and  the  associated  $’s  are 
again  calculated  with  each  cell  being  displaced  by  a.  Once  the  least  value  of  $ 


remains  in  the  center  cell  for  two  successive  searches,  then  the  displacement  of 


each  ceil,  A,  becomes 

A  =  (— )  <r  n  =  l,2,3  .  (3.25) 

2" 

The  advantage  of  this  type  of  search  is  that  it  does  not  depend  on  the 
cell  of  interest  and  its  surrounding  neighbors  being  able  to  be  fitted  with  a 
quadratic  function  and  thus  can  be  applied  to  a  larger  variety  of  terrains. 

E.  SUMMARY 

This  chapter  has  presented  the  necessity  for  an  accurate  terrain  altitude  map 
before  a  vehicle  can  become  autonomous.  After  the  data  from  an  optical  radar 
scanner  is  converted  into  a  form  that  can  be  used  for  route  planning,  there  are 
various  ways  to  compute  the  optimal  altitude  values  and  store  them  in  the  proper 
terrain  cell.  One  method  for  computing  the  optimal  altitude  values  was  presented 
along  with  two  ways  to  determine  the  proper  terrain  cell  to  store  the  altitudes.  In 
the  next  chapter,  the  simulation  model  is  presented  that  will  be  used  to  test  these 


data  manipulation  methods. 
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In  order  to  simulate  the  operation  of  the  optical  scanner,  a  simulated  terrain 
is  needed.  The  method  for  generating  and  drawing  this  terrain  is  the  same  as 
that  presented  in  Ref.  10.  An  elevation  oblique  projection  is  used  in  this  thesis  to 
display  a  three-dimensional  model  of  the  terrain  [Ref.  14:  pp. 272-316].  In  the 
elevation  oblique  representation,  lengths  and  angles  of  lines  in  a  vertical  plane  are 
preserved  while  others  are  distorted.  Figure  2  shows  the  coordinate  systems  used 
for  the  Cartesian  coordinate  system,  the  IS  I  screen  coordinate  system,  and  the 
image  coordinate  system.  Using  these  coordinate  systems,  the  following  equations 
apply: 


«  =  x  -  .5 y 


v  =  —  z  —  .by 


*  ~  +  « 


Y  =  Y„,m  -  v 

ortftn 


The  use  of  the  multiplier  value  of  1  in  Eqs.  (4.1)  and  (4.2)  means  that  each  pixel 
in  x  and  z  corresponds  to  one  inch  in  physical  units.  The  use  of  a  scale  factor  of 
.5  for  the  y  coordinate  in  both  of  the  equations  means  that  y  is  foreshortened  by  a 
factor  of  .707  relative  to  the  scale  of  x  and  z.  With  these  scale  factors,  the  terrain 
shown  in  Figure  3  is  of  dimension  64  ft.  x  64  ft.  With  Eqs.  (4.3)  and  (4.4),  the 
reference  of  the  terrain  is  set  in  the  lower  left  corner  of  the  terrain. 


<C) 


Figure  2 

Coordinate  Systems 
a)  Cartesian  ,  b)  Screen  ,  c)  Image 
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The  terrain  data  is  generated  by 

*  =  r4  cot( — - —  2 k  rS)  (4*5) 

SO 

where  r3  and  r4  are  random  numbers  and  x  and  y  are  the  horizontal  coordinates 
of  the  altitude  map  measured  in  inches.  Figure  3  is  a  picture  of  typical  terrain 
generated  by  Eq.  (4.5). 

Figure  4  is  a  flow  chart  for  the  simulation  program.  The  computer  code  in 
Appendix  A  is  commented  for  further  explanation  of  each  segment  of  code. 
Figure  5  is  a  flow  chart  of  the  simulation  of  the  optical  scanner  scanning  the 
terrain.  In  scanning  the  terrain,  the  optical  scanner’s  beam  length  is  increased 
one  pixel  (one  inch)  at  a  time.  After  each  increase  of  the  beam,  the  x,  y,  and  z 
coordinates  of  the  end  of  the  beam  are  calculated  using  the  D-H  transformation 
discussed  earlier.  The  calculated  value  of  z  is  then  compared  to  the  value  of  the 
altitude  stored  at  the  x  and  y  coordinate  of  the  actual  terrain  data.  If  the 
calculated  value  of  z  is  greater  than  the  actual  terrain  value,  the  beam  length  is 
increased  and  the  process  starts  again.  If  the  calculated  value  is  less  than  the 
actual  terrain  value,  then  the  optical  scanner’s  range  is  set  to 

*  =  -5  (4-6) 

where  is  the  range  to  the  z  value  that  is  less  than  the  actual  terrain  value 

and  the  .5  adjustment  brings  the  range  back  to  a  value  closer  to  the  actual  range. 

The  actual  ERIM  optical  scanner  scans  the  terrain  in  128  increments,  both  in 
elevation  and  azimuth.  However,  due  to  the  time  requirements  of  simulating  the 
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scanning  process,  the  simulation  of  this  thesis  uses  twelve  increments  of  one 
degree  each  in  both  the  azimuth  and  elevation. 

It  is  assumed  that  the  error  associated  with  the  the  optical  scanner  is  range 
dependent;  that  is,  the  greater  the  range  to  the  surface,  the  greater  the  error  of 
the  returned  range  value.  For  the  purposes  of  this  thesis,  this  error  will  be 
represented  by 

"L  -  °l  +  *,V  (4.7) 

where  a\ctn  is  the  total  variance  of  the  scan  and  R  is  the  range  from  Eq.  (4.6). 
Also,  lacking  any  information  about  the  scanner  errors,  a*  and  a*  are  set  at 


a o  =  .02  in 


a,  =  .04 


After  the  variance  in  the  range  measurement  is  computed,  the  new  value  of 


the  range,  R,  is  computed  by 


R  =  R  + 


(4.10) 


where  n  is  the  output  of  a  gaussian  random  noise  generator  with  zero  mean  and 


unit  variance  and 


(4.11) 


C.  DATA  HANDLING  DURING  SIMULATION 

Once  the  terrain  has  been  scanned  by  the  optical  scanner,  regression  analysis 
is  applied  to  compute  any  errors  in  the  INS  values  for  the  x  and  y  coordinates. 
Figure  6  is  a  flow  chart  of  the  gradient  search  method  of  regression  analysis. 
Using  the  INS  values  of  the  x  and  y  coordinates,  *0  is  computed  using  Eq.  (3.10). 
The  INS  value  of  the  x  and  y  coordinates  are  then  changed  by  the  values  in  Table 
3  and  the  associated  ♦’s  are  computed.  To  compute  the  4’s,  the  terrain  is 
scanned,  but  this  time  without  noise  added  to  the  range  value.  In  the  actual  ASV, 
this  would  be  done  internally  with  the  pre-stored  terrain  map  of  the  area  and  not 
an  actual  scan  of  the  terrain.  From  these  values  of  *,  Eqs.  (3.22)  and  (3.19)  are 
used  to  compute  the  k’s  and  the  H  matrix.  With  these,  Ax  and  Ay  are 
determined  by  Eq.  (3.20).  With  these  values  of  Az  and  Ay,  the  INS  values  for  x 
and  y  can  be  adjusted  by  Eq.  (3.21). 

The  second  regression  analysis  method  is  the  grid  search  method.  Figure  7  is 
the  flow  chart  for  this  search  method.  It  is  much  the  same  as  the  gradient  search 
method  except  that  the  INS  values  of  the  x  and  y  coordinates  are  changed  by  the 
standard  deviation,  <r,  of  the  INS  estimate  of  x  and  y.  After  each  cell’s  #  is 
computed,  the  minimum  value  of  ♦  is  determined,  and  if  it  is  not  in  the  center 
cell,  then  it  is  moved  to  the  center  along  with  its  associated  value  of  x  and  y.  If 
the  minimum  ♦  is  already  in  the  center  cell,  it  is  left  there.  If  the  minimum  ♦ 
was  not  in  the  center  cell,  then  after  it  is  moved  to  the  center,  the  x  and  y  values 
are  again  changed  by  a.  This  process  continues  until  the  minimum  $  stays  in  the 
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Figure  6 

Gradient  Search  Flow  Chart 


center  cell  for  two  successive  iterations.  Once  this  condition  is  satisfied,  the  x  and 


y  values  are  changed  by  aj 2*  and  the  4’s  are  again  computed.  This  process 
continues  for  n=l,2,3.  Upon  completion  of  the  search  method,  the  cell  with  the 
minimum  4  has  the  new  INS  value  of  x  and  y  to  replace  the  old  values. 

After  completion  of  the  regression  analysis,  the  Kalman  filter  is  used  to  store 
the  optimal  values  of  the  altitude  in  the  appropriate  altitude  map  cell.  The  cell 
indices  are  determined  using  the  new  INS  values  for  x  and  y  determined  by 
regression  analysis.  Figure  8  is  the  flow  chart  for  the  Kalman  filtering.  For 
initialization  of  the  Kalman  filter,  if  a  cell  in  the  altitude  map  does  not  have  a 
stored  value  for  o]u,  then  the  a\u  of  that  cell  is  set  to  be  (120)J,  which  is  the  square 
of  the  vehicle’s  height.  From  Eq.  (3.7),  the  partial  derivatives  with  respect  to  x, 
y,  and  z  are  dependent  on  the  nature  of  the  terrain.  Eqs.  (4.12)  and  (4.13)  show 
that  a\  and  are  given  by 


2 


a 


B 


(4.12) 


2 


(4.13) 


For  purposes  of  this  thesis,  the  values  of  a]  and  a\  are  set  to  1,  but  in  reality,  they 
are  dependent  on  range  as  well  as  azimuth  and  elevation  scan  angles. 


From  the  terrain  data  generated  for  this  simulation,  an  average  value  for 


( — )  was  determined  from  the  difference  in  the  altitude  value  in  the  x  direction 
dr 


Figure  8 

Kalman  Filtering  Flow  Chart 


with  the  y  value  held  constant.  From  these  calculations,  the  average  value  of  this 


quantity  is 


( — )  =  .lie  in  . 

dx 


(4.14) 


dz  .  dz 

The  average  value  for  ( — )  was  calculated  in  the  same  manner  as  that  for  ( — ) 

dy  dz 


with  the  results  the  same  as  those  in  Eq.  (4.14).  Next,  a\  is  set  to  be 
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With  the  value  of  computed,  Eqs.  (3.5)  and  (3.9)  are  applied  to  each  cell  of 
the  altitude  map  to  store  the  optimal  value  of  the  altitude.  Upon  completion  of 
the  filtering,  the  program  returns  to  scanning  the  terrain. 


D.  SUMMARY 

In  this  chapter,  the  computer  simulation  was  discussed.  The  creation  and 
scanning  of  the  simulated  terrain  and  the  scanning  of  it  was  presented  along  with 
the  two  regression  analysis  schemes  and  the  Kalman  filtering  scheme  for  handling 
the  data  obtained  from  the  scanning.  In  the  next  chapter,  the  results  of  the 


simulation  runs  will  be  discussed. 


V.  SIMULATION  PERFORMANCE 


A.  INTRODUCTION 

In  the  previous  chapters,  methods  for  correcting  INS  drift  errors  were 
presented  that  might  contribute  to  an  effort  to  make  the  ASV  an  autonomous 
vehicle.  After  development  of  a  computer  simulation  model  in  Chapter  IV, 
numerous  simulation  runs  were  needed  to  validate  not  only  the  model  but  the 
various  schemes  for  handling  data  so  that  an  accurate  terrain  altitude  map  can  be 
developed  for  future  use  in  possible  route  planning.  In  this  chapter,  simulation 
runs  are  planned  and  run  for  a  stationary  walker  and  a  walker  with  movement 
involved.  Along  with  the  results,  some  conclusions  about  these  results  are 
presented. 

B.  SIMULATION  RESULTS 

1.  Stationary  Walker  Results 

The  first  test  of  this  thesis  was  to  run  the  simulation  using  a  stationary 
ASV.  In  this  case,  the  walker  is  in  a  permanent  position  and  it  scans  the  same 
terrain  over  and  over  while  performing  Kalman  filtering  on  the  altitude 
information.  Figure  9  shows  the  simulated  terrain  with  the  area  being  scanned 
indicated  on  it.  In  this  figure,  the  optical  scanner  is  represented  by  a  small  square 
and  there  is  a  line  drawn  from  the  scanner  down  to  the  terrain  to  give  the  viewer 


an  idea  of  the  scanner’s  relative  position  over  the  terrain.  Also,  the  INS  origin  is 
set  to  be  the  lower  left  hand  corner  of  the  terrain.  Due  to  the  assumptions  made 
in  Chapter  III,  Section  C,  the  x  coordinate  increases  positive  to  the  right  from  the 
INS  origin  along  the  horizontal  axis,  and  the  y  coordinate  increases  negative  along 
the  other  (diagonal)  axis. 

To  evaluate  the  effectiveness  of  the  Kalman  filtering  presented  in  this 
thesis,  three  terrain  cells  are  looked  at  to  see  how  the  optimal  altitude  value 
changes  as  the  terrain  is  scanned  in  a  stationary  position.  To  get  a  representation 
of  how  the  filtering  is  working  at  different  ranges,  the  three  terrain  cells  are 
selected  at  scanner  elevations  of  -26,  -41,  and  -61  degrees.  Figure  10  is  a  plot  of 
the  three  terrain  cells  altitudes  stored  after  each  scan  after  Kalman  filtering  has 
been  applied.  To  check  that  the  filtering  works  no  matter  what  the  noisy  range 
values  returned  by  the  optical  scanner  are,  a  second  simulation  was  run  on  the 
same  terrain  in  the  same  position.  Figure  11  shows  the  results  obtained  for  the 
same  three  terrain  cells. 

The  next  test  made  on  the  stationary  walker  was  done  with  the  scanner 
in  a  different  position  on  the  terrain  and  pointing  in  a  different  direction.  Figure 
12  shows  the  scanned  terrain  for  this  case.  Figure  13  shows  the  results  of  the 
Kalman  filtering  on  three  terrain  cells  at  the  same  elevation  angles  used  in  the 
first  case.  Figure  14  shows  the  same  three  terrain  cells  with  different  noisy  optical 


scanner  range  values. 


The  last  simulation  conducted  for  the  stationary  walker  case  was  to  scan 
the  same  terrain  shown  in  Figure  12  and  let  the  scanner  scan  it  for  40  scans. 
Figure  15  shows  the  results  of  the  40  scans  for  the  same  terrain  cells  looked  at  in 
Figure  13. 

2.  Moving  Walker  Case 

The  next  case  to  look  at  was  the  walker  with  motion  involved.  To  get 
the  optimal  altitude  value  for  each  terrain  cell,  errors  in  the  INS  values  of  the  x 
and  y  coordinates  must  be  corrected  for  so  the  altitude  value  can  be  stored  in  the 
correct  cell.  For  the  purposes  of  this  thesis,  the  net  effects  of  walker  motion  are 
represented  by  an  INS  initialisation  error  or  offset. 

The  first  method  used  to  correct  for  the  INS  errors  was  the  gradient 
descent  method.  The  scanner  was  placed  in  the  same  position  as  represented  in 
Figure  9  and  given  an  initialization  error  of  Ax  =  5  inches  and  Ay  ■=  -4  inches.  After 
the  first  scan,  gradient  descent  was  applied  using  Eq.  (3.20),  and  the  optimum 
values  for  A x  and  Ay  were  10  and  5  respectively.  A  second  run  was  conducted 
with  different  INS  errors  and  the  results  were  again  not  close  to  the  value  they 
should  have  been.  The  reasons  for  this  failure  are  not  known,  but  appear  to  be 
associated  with  the  inadequacy  of  a  quadratic  approximation  to  the  squared  error 
function,  *.  This  problem  might  be  solved  by  a  more  elaborate  gradient  descent 
method  as  discussed  in  Ref.  8,  but  this  approach  was  not  investigated  in  this 


After  the  apparent  failure  of  the  gradient  descent  method,  the  grid  search 
method  was  applied  to  the  moving  walker  case.  The  first  simulation  run  using 
the  grid  search  method  was  with  the  scanner  in  the  position  indicated  in  Figure  9 
and  with  the  standard  deviation  of  the  INS  offset  given  by,  a  =  6  inchet.  This  a 
was  an  assumption  and  is  used  in  the  terrain  cell  mask  described  in  Chapter  III, 
Section  D.  To  test  the  effectiveness  of  the  grid  search  method,  a  run  was 
conducted  with  the  INS  error  less  than  a,  another  run  with  the  error  between  a 
and  2 a,  and  a  third  run  with  the  error  greater  than  2 a.  Figure  16a  shows  the 
results  of  the  regression  analysis  with  the  INS  error  less  than  a.  The  plot  shows 
the  actual  x  and  y  values,  the  starting  point  of  the  INS  x  and  y  coordinates  and 
the  numbers  indicate  the  steps  of  the  grid  search.  The  final  value  is  the  value 
returned  to  be  used  in  the  Kalman  filtering  by  the  grid  search  at  the  end  of  the 
first  actual  terrain  scan.  As  Figure  4  shows,  the  regression  analysis  is  repeated 
each  time  a  new  frame  of  noisy  range  values  is  obtained  from  the  simulation. 


Figure  16b  shows  the  results  of  the  grid  search  after  the  first  scan  with  the  INS 
error  between  a  and  2a.  After  the  second  scan,  the  data  showed  that  the  grid 


search  method  locked  in  on  the  actual  x  and  y  coordinates  and  stayed  there  for 


subsequent  scans.  Figure  17  shows  the  results  of  the  grid  search  after  the  first 
scan  with  the  INS  error  greater  than  2a.  The  data  collected  also  shows  that  the 
grid  search  method  would  not  lock  in  on  the  actual  x  and  y  values  on  the 
succeeding  scans  when  the  error  was  greater  than  2a. 
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Grid  Search  Results  With  a  -  6  inche* 
a)  INS  Error  <  a  ,  b)  a  <  INS  Error  <  2 a 
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Figure  17 

Grid  Search  Results  With  <7  =  6  inches 
INS  Error  >  2 a 


Upon  completion  of  each  grid  search,  Kalman  filtering  was  applied  to 
obtain  the  optimal  altitude  value  for  each  terrain  cell.  Figure  18  are  the  results  of 
the  Kalman  filtering  after  8  scans  with  the  INS  error  less  than  a.  The  data  also 
shows  that  the  results  of  the  Kalman  filtering  when  the  INS  error  is  between  a 
and  2 o  are  almost  exactly  the  same  as  those  shown  in  Figure  18.  Figure  19 
presents  the  results  of  the  Kalman  filtering  when  the  INS  error  is  greater  than  2a. 

The  next  test  of  the  grid  search  method  was  to  set  the  value  of 
a  =  12  inchet.  With  this  value  of  a ,  the  same  type  of  runs  were  conducted  as  when 
a  =  6  inches.  Figure  20  shows  the  results  when  the  INS  error  is  less  than  2a.  Figure 
20a  shows  the  grid  search  locks  in  on  the  actual  x  and  y  values  with  zero  error 
after  the  first  scan.  Figure  20b  indicates  that  the  grid  search  did  not  lock  in  on 
the  actual  values  after  the  first  scan.  To  further  test  the  grid  search  with  the  INS 
error  between  a  and  2a,  another  run  was  conducted  with  different  errors  in  that 
range  and  the  results  are  shown  in  Figure  21.  The  data  showed  that  the  grid 
search  locked  in  with  zero  error  on  the  actual  position  on  the  first  scan  and  stayed 
locked  in  on  all  succeeding  scans.  The  final  run  was  conducted  with  the  INS  error 
greater  than  2a  and  those  results  are  shown  in  Figure  22.  As  with  the  results 
when  <t  =  6  inches,  the  grid  search  would  not  lock  in  on  the  actual  position  when 
the  INS  error  was  greater  than  2<r.  When  the  Kalman  filtering  data  was  reviewed, 
the  results  were  very  much  like  that  received  when  a  =  6  inches. 

So  far  all  of  the  results  for  the  moving  walker  case  involved  using  a  pre¬ 
stored  terrain  altitude  map.  One  final  simulation  conducted  was  to  see  if  the  grid 
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Figure  23 

Grid  Search  Results 
No  Pre-stored  Altitude  Map 


search  method  would  work  without  a  pre-stored  map.  In  this  case,  the  scanner 
was  placed  in  the  position  represented  in  Figure  9  and  the  terrain  was  scanned  20 
times  to  obtain  an  altitude  map.  From  previous  results,  it  is  known  that  the  map 
stored  for  the  area  scanned  was  very  close  to  the  actual  map  and  the  data  for  this 
case  also  showed  that  to  be  true.  Upon  completion  of  the  20  scans,  the  scanner 
was  given  an  offset  and  then  regression  analysis  and  Kalman  filtering  was  applied 
for  the  next  8  scans.  Figure  23  shows  the  results  of  the  grid  search  after  the  first 
scan.  As  can  be  seen,  the  grid  search  went  farther  away  from  the  actual  position. 
Actually,  the  grid  search  only  conducted  3  steps  because  of  the  safety  factor  built 
into  the  program  so  it  would  not  look  forever  if  it  was  not  converging.  Also,  the 
data  indicated  that  on  the  succeeding  scans,  the  grid  search  actually  got  worse. 
This  run  was  conducted  with  an  a  »  6  ineha  and  the  offset  between  a  and  2 a.  It 
was  run  again  with  the  offsets  less  than  a  and  the  results  were  the  same. 

C.  CONCLUSIONS 

The  first  general  conclusion  to  be  made  from  the  results  presented  in  this 
chapter  is  that  the  Kalman  filtering  is  working.  As  can  be  seen  from  the  results  of 
the  stationary  walker  case,  the  stored  altitude  values  were  very  close  to  the  actual 
values.  For  the  terrain  cells  where  the  scanner  was  at  -26  and  -61  degrees,  the 
stored  values  were  within  .25  inch  of  the  actual  values  while  the  one  at  -41 
degrees  was  within  about  1.1  inches.  To  evaluate  the  significance  of  these  results, 
data  was  obtained  concerning  the  standard  deviation  of  the  raw  terrain  elevation 
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values  obtained  from  the  noisy  range  values.  The  observed  standard  deviation 
had  a  value  of  2.95  inches  when  averaged  over  all  cases,  much  greater  than  the 
filtered  values.  One  possible  reason  why  the  stored  values  at  -41  degrees  had  a 
greater  error  was  the  arbitrary  range  adjustment  from  Eq.  (4.6)  and  the  sin2  effect 
of  Eq.  (4.15).  In  any  case,  these  results  would  probably  be  good  enough  for  some 
route  planning  program  to  use. 

Referring  to  Figure  15,  an  apparent  bias  in  terrain  altitude  estimates  is 
revealed.  From  the  magnitude  and  general  behavior  of  this  bias,  it  is  believed 
that  it  results  from  the  quantization  of  range  into  increments  of  one  inch.  This 
quantization  is  modulated  by  the  sin2  term  of  Eq.  (4.15),  leading  to  the  differences 
between  the  three  curves  of  Figure  15. 

The  next  conclusion  is  that  the  gradient  descent  method  of  regression  analysis 
was  not  effective  with  this  simulated  terrain.  The  reason  for  this  failure  is  not 
known  except  it  may  have  something  to  due  with  the  terrain  and  the  resulting 
"lumpy”  error  criterion  function,  0. 

The  overall  conclusion  about  the  grid  search  method  is  that  it  is  effective. 
How  well  it  works  depends  on  the  a  selected  and  the  amount  of  INS  error  relative 
to  that  a.  As  Figure  20b  showed,  with  a  INS  error  of  Az  =  21  inches  and 
Ay  =  19  inches,  the  grid  search  would  not  lock  in  on  the  actual  position  but  as  seen 
in  Figure  21,  with  an  error  of  A z  =  18  inches  and  Ay  =  -17  inches ,  the  grid  search 
locked  in  on  the  actual  position.  This  would  indicate  that  there  might  be  a  limit 
to  the  amount  of  INS  error  involved  before  the  grid  search  can  correct  for  the 
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error  which  is  independent  of  the  value  of  a.  With  this  method  though,  the  INS 
initialization  errors  or  offset  can  be  corrected  for,  within  limits,  and  thus  allowing 
the  Kalman  filtering  to  provide  an  accurate  terrain  altitude  map  when  a  pre¬ 
stored  altitude  map  is  available. 

Finally,  using  the  grid  search  method  without  a  pre-stored  altitude  map  did 
not  work.  In  analyzing  the  results,  a  possible  reason  for  its  failure  was  because 
the  altitudes  in  all  of  the  surrounding  terrain  cells  was  initialized  to  zero  so  when 
the  terrain  mask  was  applied  and  the  stored  map  scanned,  the  altitude  values 
stored  in  the  resulting  Cartesian  coordinate  map  would  probably  be  less  than  they 
should  have  been,  thus  giving  a  minimum  criterion  function  in  a  false  cell  during 
regression  analysis.  With  this  false  minimum,  the  grid  search  would  chase  after 
the  wrong  cell. 
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VI.  SUMMARY  AND  CONCLUSIONS 

A.  RESEARCH  CONTRIBUTIONS 

In  this  thesis,  a  first  approach  was  presented  in  providing  an  optimal  terrain 
altitude  map  that  could  be  used  for  route  planning  for  an  autonomous  walking 
machine.  Toward  that  end,  an  effective  Kalman  filtering  scheme  was  developed 
and  tested.  The  results  of  those  tests  show  that  the  altitude  values  stored  in  the 
terrain  map  are  good  enough  for  a  walking  machine  to  walk  across. 

Another  important  part  of  providing  an  optimal  altitude  map  is  to  ensure 
that  the  altitude  values  are  stored  in  the  right  terrain  cell  locations  associated 
with  the  proper  x  and  y  coordinates  of  the  terrain  area  being  traversed.  If  the  x 
and  y  coordinates  are  in  error  because  of  errors  in  the  inertial  navigation  system, 
then  it  is  necessary  to  correct  for  those  errors  before  storing  the  output  of  the 
Kalman  filter.  To  solve  this  problem,  a  regression  analysis  approached  called  the 
grid  search  method  was  presented.  This  search  method  proved  to  be  effective  in 
correcting  for  these  land  navigation  errors  on  the  simulated  terrain  within  limits. 
These  limits  appear  to  be  within  about  18  inches  of  the  actual  x  and  y  coordinate 
values  with  the  terrain  used  in  this  thesis.  For  a  first  approach,  this  type  of 
accuracy  is  good  and  provides  for  a  satisfactory  starting  point  for  refining  this 
method. 
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Another  contribution  of  this  thesis  was  to  provide  a  simulation  for  terrain  and 
an  optical  terrain  scanner  scanning  that  terrain.  This  simulation  was  developed 
for  the  ISI  graphics  workstations  and  could  be  helpful  for  any  further  work  that  is 
conducted  on  these  or  similar  workstations. 

B.  RESEARCH  EXTENSIONS 

Since  the  topics  presented  in  this  thesis  represent  a  first  attempt  at  optical 
radar  data  filtering  with  the  ASV  in  mind,  there  are  many  areas  that  can  be 
expanded  upon. 

First,  further  research  is  needed  into  why  the  gradient  descent  method  did  not 
work.  It  is  very  possible  that  the  reason  has  something  to  due  with  the  simulated 
terrain  used.  Another  extension  that  could  be  pursued  would  be  with  the  grid 
search  method.  Along  with  having  a  decreasing  mask  in  the  grid  search,  an 
expanding  mask  could  be  developed  if  the  minimum  criterion  function  cannot  be 
brought  into  the  center  of  the  grid.  Also,  further  research  is  needed  to  determine 
the  optimal  value  for  the  grid  size  to  use  in  the  mask  and  what  limits  there  are  on 
the  errors  that  can  be  corrected.  Along  with  this,  the  grid  search  method  should 
be  applied  to  completely  different  terrain  and  the  optimal  value  of  grid  size 
determined  and  then  compared  to  the  one  for  this  terrain  to  see  if  there  is  a 
relationship.  Next,  investigation  into  ways  to  use  the  grid  search  method  to 
correct  for  errors  introduced  by  the  accelerometers  while  walking  would  allow  the 
vehicle  to  continuously  walk  while  the  grid  search  method  corrects  its  position. 


Currently,  under  the  scheme  presented  in  this  thesis,  the  vehicle  would  have  to 
take  a  few  steps,  stop  and  correct  its  position,  and  then  take  a  few  more  steps. 

Another  important  area  that  needs  to  be  studied  is  the  one  of  applying  these 
techniques  to  the  moving  walker  case  where  no  pre-stored  altitude  map  is 
available.  As  seen  in  Chapter  V,  the  first  attempt  involving  this  situation  did  not 
work.  Research  into  why  it  did  not  work  and  how  to  make  it  work  will  be 
invaluable  to  making  the  walking  machine  autonomous. 

The  last  area  that  should  be  studied  is  to  apply  the  techniques  that  have 
proven  successful  in  simulation  to  actual  terrain  with  actual  optical  scanner  data. 
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APPENDIX 


PROGRAM  LISTING 


/****4******************««*** *********************** 

This  is  &  computer  simulation  of  an  optical 
scanner  scanning  simulated  terrain  using 
regression  anaylsis  and  Kalman  filtering. 

This  program  is  written  in  C  for  the  ISI 
graphics  workstations. 

This  program  is  compiled  by 

cc  scan.c  -ltools  -lbm  -lvt  -1m 

by  Mark  D.  Rickenbach 
1  August  1987 

***************************«*************«**********/ 

finclude  <math.h> 

#include  <vt.h> 

#include  <stdio.h> 

^include  <bitmap.h> 

§  include  <tools.h> 
struct  BMD  *bmd; 
int  seedl; 

float  seed,seed2,a0_9[4)[4]1seed3; 
main() 

{ 

static  int  u,v1u2,v2,scan1iijj1v21,u21,rr,f,initlx,y,s,k; 
static  int  randl,rand2, randS, rand4,xint,yint, safe, counter; 
static  int  bmyl,bmxl,bmx,bmy,fd,i  j,kk,xx,yy,s,rang,l; 
static  float  sigscan,sig02,sigl2,array(l28][l28],d9,rj4S6]; 
static  float  n.sigzold2(l28][128|,noise(),sigscan2,elev,dx[9|,dy[9j; 
static  float  xxl,yyl,zzl,d3min,gmin,zhat[128j[l28],ss|128]il28); 
static  float  w,tl,celev,melev,sigold2[l28|[128j,sigz2[128|[l28j; 
static  float  th4,th5,th6,dl,d2,d3,th7,th8,g(8],rhat[456j,d2min; 

/*** . . . . . * . 

Open  window,  allocate  bit  map  and  set  line  discipline  to  graphics 

for  graphics  simulation  ASV  scanning  terrain. 

•  *  *  •**  *  *•••  ••***•***•••****  *»  »  ••  •***•••*•••••, 


fd=Open Window(  15, 63, 1220, 664, "Terrain  Map”): 

SetLineDisc(fd.TWSDlSC); 

if  ((bmd=BM  Allocate(  1220,664))  =  =0) 

{  printf("  unable  to  allocate  bit  map"), 

} 

BMSet  Addressing(bmd.l); 

BM  SetColor(bmd,l); 

BM  Set BColor( bmd  ,0) ; 


I 


r . ************ . 

Draw  terrain  to  bit  map  using  scheme  developed  by 
Poulus  in  his  thesis.  Each  terrain  cell  will  be 
represented  by  a  6  in.  x  6  in.  square. 


i=0; 

1=0; 

loop: 

x=i*6; 

y=j*«; 

i=array[j](i]; 

bmx=xtrfune(&xint,&x,&y); 

bmy=ytrfunc(lcyint,icy,&s); 

loopl: 

if(i==127  kb  j==127) 

{  BM_SetThickness(bmd,2); 
bmyl=bmy+s; 

BM_PaintLine(bmd,bmx,  bmy  1 ) ; 
BM_SetPosition(bmd,bmx,bmy); 

BM_SetThickness(bmd,l); 

BM_DisplayBitmap(fd,PAINTR  ASTER, bmd, 0,0, 0,0, 1220, 664,0); 
goto  joop2; 

} 

if(j==127) 

{if(i==0) 

{  x=0; 
y=j*6, 

bmx=xtrfunc(&xint,&x,fey); 

bmy=ytrfunc(&yint,&y,&*); 

BM_SetPosition(bmd,bmx,bmy); 

} 

BM_SetThickness(bmd,2); 
bmy  l=bmy+i; 

BM_PaintLine(bmd,bmx,bmyl); 

BM_SetPosition(bmd,bmx,bmy); 

BM_SetThickness(bmd,l); 

i=i+l; 

x=i*6; 

y=j*0; 

*=arrayj]|i]; 

bmx  =  xtrfunc(fcxint,&x,&y); 
bmy  =ytrfunc(4£yint,4£y,<£z); 

BM  PaintLine(bmd,bmx,bmy); 
goto  loopl; 

) 

if(i  -  -  127) 

{  BM  SetThickness(bmd,2); 
bmy  1  -  bmy 

BM  PaintLine(bmd,bmx,bmyl); 

BM  SetPo«ition(bmd,bmx,bmy); 

BM  SetThickness(bmd,l ); 


j=j+i; 

x=i*6; 

■=MT»ylj||i|; 

bmxl=xtrfanc(lcxinl,4x,lty) 

bmyl=ytrfiwc(lcyiat,lcy,lcs); 

BM  PuatLia«(bmd,bmxl,bmy  1), 

»=<>f 

goto  loop; 

} 

if(»=“0||  j==0) 

{  if(i==0  tctc  j= =0) 

{BMS«tTkicknem(bind,2). 
BM_PaintLiB«(bmd,bmx.bmy ) ; 
goto  loopt; 

} 

x=i*8; 
i— •rrmylj]|i); 

bmx=xtifmBc(lcxiat, it  x, icy); 
bmy=ytrfuBc(ityint,icy,ici), 

BM  _S«t  Posit  ion  ( bmd ,  bmx ,  bm  y ) , 

BM  _S*tTh  ickB«w(  bmd ,  2 ). 
bmyl=bmy+«; 

BMPainiLm*(  bmdbmx.bmy  1 ) , 
loopt : 

BM_SetPoMtiofl(bmd,bmx,bmy), 

BM  SetThKkii«M(bmd.l) 

i=i+l; 

x=(^I)*«; 

y=(j*l)VJ; 
s = BiTBy  Li  •+  lj|»-l|; 
bmxl=xtrfttnc(ltxiBt,lcx,Icy); 
bmyl=ytrfvBc(lcyiBt,lcy,lci); 
BMPuntLine(bmd,  bmx  1 ,  bm  y  1 ) . 

BM  SctPositioB ( bmd , bmx , bmy ) ; 
x=i*6; 

y=j*«; 

*-*rr*yLi]|'|; 

bmx=xtrfunc(ltxiBt,itx,ity); 

bmy=ytrfuBc(ltyiBt,lcy,lei); 

BM  PaintLiae(bmd,bmx,bmy), 
goto  loopl; 

} 

i=i+l; 

x  =  (i-l)*©; 

y=U+i)*«; 

*=«rr»y(j-4-  ljji-l{; 
bmxl=xtrfunc(icxint,itx,i£y); 
bmy  l=ytrfunc(Icyint,icy,it*); 
BM_P4intLine(bmd,bmxl,bmyl); 
BMSetPoxitionfbmd.bmx.bmy); 
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x=i*8; 

y-i*«; 

*=Mrr*y[j)l*l; 

bnu  *  xtrfmnc  ( It  xiat, lex, Ity ) ; 
bmy«yiifMc(liyiai,liy,lii); 
BM  PaialLinc(bmd,bmx,bmy ) 
goto  loopl. 
loop]: 


Eater  the  •tartutg  LANS  information  from  keyboard. 

. / 

pnatfC  eater  x -cooed  of  ASV  0); 

•caa/r*r.*d2); 

pnatf("  eater  y -coord  of  ASV  0), 

•caafCW.IcdS), 

pnatfC  eater  s-coord  of  ASV  0), 

scanfCSfMtdl), 

pnatf("  eater  scanner  body  aimuith  aagle  0); 

•eaaf("*r,itt4|; 

pnatfC  eater  scanner  body  elevation  aagle  0); 

*caaf("%r  Itt5), 

prtatfC  eater  scanner  body  roll  angle  0); 

•cnnfr'VMcte). 

•can  =0. 
xxl  =d2; 
yyi=d3; 
ill  =dl. 

d2=d2^29;  /*  Introduce  LANS  initialisation  errors  if  any  */ 
dJ=dJ-J0; 


Start  scanaing  terrain. 

. / 

do  { 

scan -scan*  li 
print f( "  scan  %d  n,scan); 
u2-(xxl-0.5*yy  I-*-30), 
v2=  (ill  ♦  ,5*yy  1  +  542); 


Draw  a  rectangle  to  indicate  position  of  the  ASV  scanner. 

. */ 

BM  SetPosition(bmd,u2,v2); 

BM  Set  Addressing)  bmd  ,0) ; 

BM_PaintRectanglelnterior(bmd,8,8); 

BM  Di*playBitmap(fd,PAINTRASTER,bmd,O,O,O,O,122O,064,O); 

BM  Set  Addressing ( bind ,  1 ) ; 

BM  SetThickness(bmd,2); 


u21=xxl-.5*yyl+SO; 

v21=.5*yyl+542; 

BM  PaintLine(bmd,u21,v21); 
BM_SetPosition(bmd,u2,v2); 
BM  SetThickness(bmd,l); 


/••••a************************************************* 

Scan  terrain  with  one  full  asmuith  scan  for  each  elevation 
increment  starting  at  elevation  represen  td  by  elev  and 
scanning  down.  For  init=l,  this  simulates  scanner  operation 
returning  a  noisy  range  value,  rjrrj.  For  init=2,  this 
simulates  the  ASV’s  computer  internally  scanning  its 
pre-stored  terrain  map  and  returning  noiseless  range  values, 
rhatjrr].  For  the  purposes  of  this  thesis,  the  scanner 
scans  in  three  elevation  segments  of  12  degrees  each  at 
•20,  -35,  and  -55  degrees. 


for(init=l;init<3;init++) 

{rr=0; 

i=i; 

do  { 
if(i==l) 

{elev=(-l)*20.; 

} 

else  if  (i==13) 

{elev=(-l)*22.; 

} 

else  if  (i==25) 

{elev=(-l)*30; 

} 

th7=elev-i; 

tt=l; 

j=i; 

do  { 

rr=rr+l; 
th8=6-j; 
if(tt— =1) 

{rang=0; 
do  { 

rang=rang+l; 

d9=rang; 

computematrix(txxl,tyyl,&zsl,i£th4,l£th5,<:th8,&th7,&th8,&d9); 

l=(a0_9|l)(4]/6); 

k=(-l)*a0_9[2]|4j/6; 

celev=(-l)*  (a0_9[3]j4]); 

melev =array  [  k]  [  1] ; 

}  while(celev>melev); 

} 


d9=rang; 

computematrix(&xxl,&yyl,&ssl,&th4,&th5,&th6,&th7,&th8,&d9); 

l=(aO_9(l][4]/«); 

k=(-l)*aO_9[2)[4]/«; 

celev=(-l)*  (a0_9|3][4]); 

melev= array  (k)[l); 

}  while(celev  < =melev) ; 
do  { 

rang=rang+l; 

d9=rang; 

computematrix(&xxl,&yyl,&»l,&th4,&th5,&th6,&th7,&th8,&d9); 

l=(aQ_9(l]|4j/«); 

k=(-l)*m0_9j2]|4j/e; 

celev=(-l)*  (a0_9(3][4|); 

melev=array[k](l|; 

}  while(celev>melev); 

} 

d9=d9-.5;  /*  back  up  range  by  .5  inch  */ 

u=(a0_9[l][4]-0.5*a0_9[2][4|+30); 

v=(0.5*a0_9|2||4]+a0_9|3][4)+542); 

BM^SetPosition(bmd,u2,v2); 

BM_PaintLine(bmd,u,v); 

BM  DisplayBitmap(fd,PAINTR  ASTER, bind, 0,0, 0,0, 1220, 664,0); 


Compute  the  noisy  range  value  for  init=l. 


sig02=0.02; 

sigl2=0.0001; 

n=noise(); 

sigscan2=sig02  +  sigl2*d9*d9; 
sigscan =sqrt  (sigsc  an2 ) ; 
if(init  =  =  1) 

{n=noise(); 
d9=d9-f  n*sigacan  ; 
r|rr]=d9; 

} 

else 

{rhat|rr]=d9; 

} 

tt=tt+l; 

j=j+l; 

}  while(j<  12); 
i=i+l; 

}  while(i<36); 

} 


/ 


•1m  if  (u»-») 

{dl-dt+./f; 

dl«dS+t/f; 

) 

rr»0; 

i-l; 

do  { 


•Im  if  (i««  IS) 

) 

*1m  if  (i==25) 

) 

tk7~*Uv-i; 

U-i, 

j=i; 

do  { 

rr=rr+l, 

tk»=6.j; 

«n**==u 

{r%m*0; 
do  { 

ru|-ru|-fl; 

d9>ru|; 

compvuaitl  rix(ltdl,<td2,4td3,  It  t4.  IctS,  Ici6,luk7.itth8.4(d9|. 

l-(<0  9|l||«|/«); 

k-(.l)*«0  9|J||*)/6; 

c«I«y»(-I)*  («0_9|»||4|); 

m«iev=wray[ki|l|; 

}  wkilc(ccl«v>melcv); 

) 

•l*e 

{ 

do  { 

ring =r*n|-l, 
d9=r*ng; 

computem*trix(Jtdl>ftd2,Jcd3,4ct4,fctSlltt6,Jah7,fcih8,fcd9): 

l=(^J  9|l||4|/6); 

k=(-l)**0  9|2j|4]/9; 

cel«v=(-l)*  (*0_9|S||4|); 

melev=arr*y|kj|l); 

}while(cel«v<=melev); 
do  { 

ruig-rang+1; 

d9=rang; 

computematrix(icdl,4£d2,<dd3,idt4,4:t5,i’t6,ith7,4ith8,A:d9), 

l=(aO_9(I|(4)/«); 

k«(-l)*.0_9|2|[4]/6; 

celev=(-l)*  (aO_9(3!(4]); 


}  wliiW(c«irv  >m«irv), 

> 

49-49-  ft; 
rk*tlir!*<i9; 

U«U*I, 

)  whiWO<  13) 
i«»+  I, 

}  whikO'M). 


CoMpitf  rnlirioa  f«KlK»  for  tke  ippropntt* 
c 40  mask 

i 

|i»»l=0. 

fof(i(-lj)<  433  ■*  ) 

{|  u  =tl‘»i*  (r'JJ  -rfc*tl  jj|)*(rjj|-rhMl[jj|), 

) 

dxfu  -47, 
djrjui=dS. 
tf|u— 1) 

(dJ-dl-tf. 

d»-d>v,f 

> 

aka  if  (ti*  =  2) 

(dl*dJ  +  i/f. 

} 

ilw  if  (u^zS) 

(da-da-t/f 

dl=dS-*«,  f. 

) 

eke  if  (ii  =  =4) 

{d3*da**/f 

} 

eke  if  ( ai~  =5) 

{da=da-./f, 

) 

eke  if  (»i=  =0) 

{da=da-»/f; 

dJ=dJ-»/f; 

} 

eke  if  (ii=  =  7) 

{dS=d3-./f; 

) 

eke  if  (ii==8) 

{da=d3-*/f; 

dS=d3*«/f; 

} 

} 
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/ 


{elev=(-l)*20.; 

} 

else  if  (i==13) 

{elev=(-l)*22.; 

} 

else  if  (i==25) 

{elev=(-l)*SO.; 

} 

th7=elev  -i; 
j=l; 
do  { 

th8=ft-j; 

rr=rr+l; 

d9=r[rr]; 

computematrix(&xxl,&yyl,&ssl,&th4,&th5,&th6,&th7,&th8,&d9); 

xx=«0_9{ll|4l/«; 

yy=(-l)*a0_9|2||4]/«; 

ssIxx]|yy)=(-l)*aO_9[S)|4|; 

/ . * . * . ****** 

Perform  Kalman  filtering  for  each  terrain 
cell  scanned. 

. . . . . * . / 

sigs2[xx][yy|=(sig02  +  sigl2*d9*d9)*sin(th7*3.  14159/180) 

*sin(th7*3. 14159/180)  +  . 232; 
if(sigiold2|xx][yy]==0) 

{sigsold2(xxj[yy)= 144000.  ; 

} 

ihat[xx][yy)=ihat[xx][yy|+(sigiold2jxx|[yy|/(sigsold2[xx]|yy]+sig*2[xxj[yy])) 

*(ss|xx|(yy]-shat(xx)|yy)); 

sigiold2|xx][yy|=sigi2|xx]|yy]*9igiold2jxx]|yy]/(sigs2[xx]jyy)+sigsold2|xx][yy]); 

j=j+i; 

}  while(j<12); 

i=i+l; 

}  while(i<36); 

}  while(scan<8); 
printf("  program  complete  "); 
while  (1) 

{} 

} 

^**************4******************4s*********************** 

End  of  the  main  program 

•**********************************************************y 


!**  ************************************************* ******** 
Subroutine  to  perform  the  coordinate  transformation 
using  the  D-H  transformation.  Only  the  values  of  the 
matrices  needed  are  computed. 

ft********************************* **************************  j 
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eomputematrix(xl,yl,Bl,tth4,tth5,tth6,tth7,tth8,dd9) 
float  *xl,*yl>**l>*tth4,*tth5)*tth6,*tth7,*tth8,*dd9; 

{ 

float  c4,c5,c6,c7,c8,thact4,thact5,thact6,th7,th8,d9; 

float  a4,s5,s6,s7,s8,xxl,yyl,ssri 

int  a7; 

xxl=(*xl); 

yyl=(*yl); 

«*  l— (*»i); 

thact4=(*tth4); 

thact5=(*tth5); 

thact6=(*tth6); 

th7=(*tth7); 

th8=(*tth8); 

d9=(*dd9); 

c4=cos(thact4*3. 1416/180  +3.1416); 
e5=eos(thact5*3. 1416/180  -1.5708); 
c6=cos(thact6*3. 1416/ 180  +3.1416); 
c7=cos(th7*S.  1416/ 180  -1.5708); 
c8=cos(th8*3. 1416/180  -1.5708); 
s4=sin(thact4*3. 1416/ 180  +  3.1416); 
s5=sin(thact5*3. 1416/180  -1.5708); 
s6=sin(thact6*3. 1416/180  +  3.1416); 
s7=sin(th7*3. 1416/180  -  1.5708); 
s8=sin(th8,‘3. 1416/180-  1.5708); 

•7=(*l)*5; 

a0_9[l][4j=xxl+(c4*c5*c6+s4*«6)*(d9*c7*s8+a7*c7)+(c4*85)*(d9*s7*»8+a7*87) 

+(c4*c5*s6-84*c6)*(c8*d9); 

a0_9[2][4]=yyl+(s4*c5*c6-c4*s6Wd9*c7*a8+a7*c7)+(s4*s5)*(d9*s8*s7+a7*s7) 

+(s4*c5*s6+c6*c4)*(c8*d9); 

a0_9(3||4]=iil+(s5*c6)*(d9*c7*s8+a7*c7)-(d9*s8*s7+n7*s7)*(c5) 

+(s5*s6)*(c8*d9); 

} 


/ 


Subroutines  to  do  x  and  y  transformations  used  in  the 
terrain  drawing  portion  of  the  program. 


/ 


xtrfunc(xint,x,y) 
long  int  *x,*y  ; 
int  *xint; 

{ 

int  xx; 

xx=(*xint)  +  (*x)  +  (*y)*.5; 
return  xx; 

} 


ytrfunc(yint,y,z) 
long  int  *y,**  ; 
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/ 


Random  number  generator  need  in  creating  terrain. 


ran(r) 
int  r; 

{ 

int  ran; 

»eed=(eeed2-»-3.1)  *  (eeed2+3  1), 
eeedl=*eed; 

•eed  2 = aeed-aeed  1 ; 
ran  =  (r  )*(eeed2)  +  i; 
return  (ran); 

} 


/ . 

Subroutine  uaed  to  create  a  random  gauaaian  number 
with  mean=0 


float 

noiee() 

{ 

float  b,n,eeed,x,xx; 
int  i.eeedi; 
b=0.5; 
xx=0; 

for(i=l;i<13;-*-  +  i) 

{ aeed=  (eeedS-t-S.  141634)  *(eeed3+ 3. 14 1634); 

teedl=eeed; 

eeed3=seed  -  seedl; 

x=2*#eed3  -  1; 

xx=xx  +  x; 

} 

n=b*xx; 

return(n); 
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